#!/usr/bin/env python

from dynamic_reconfigure.parameter_generator_catkin import *

# from local_planner_limits import add_generic_localplanner_params

gen = ParameterGenerator()

# This unusual line allows to reuse existing parameter definitions
# that concern all localplanners
# add_generic_localplanner_params(gen)

# For integers and doubles:
#   Name        Type  Reconfiguration level
#   Description
#   Default  Min  Max


grp_trajectory = gen.add_group("Trajectory", type="tab")

# Trajectory
grp_trajectory.add("teb_autosize", bool_t, 0,
                   "Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)",
                   True)

grp_trajectory.add("dt_ref", double_t, 0,
                   "Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)",
                   0.3, 0.01, 1)

grp_trajectory.add("dt_hysteresis", double_t, 0,
                   "Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref",
                   0.1, 0.002, 0.5)

grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0,
                   "Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically",
                   True)

grp_trajectory.add("allow_init_with_backwards_motion", bool_t, 0,
                   "If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)",
                   False)

grp_trajectory.add("max_global_plan_lookahead_dist", double_t, 0,
                   "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]",
                   3.0, 0, 50.0)

grp_trajectory.add("force_reinit_new_goal_dist", double_t, 0,
                   "Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)",
                   1.0, 0.0, 10.0)

grp_trajectory.add("force_reinit_new_goal_angular", double_t, 0,
                   "Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)",
                   0.78, 0.0, 4.0)

grp_trajectory.add("feasibility_check_no_poses", int_t, 0,
                   "Specify up to which pose (under the feasibility_check_lookahead_distance) on the predicted plan the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_lookahead_distance are checked.",
                   5, -1, 50)

grp_trajectory.add("feasibility_check_lookahead_distance", double_t, 0,
                   "Specify up to which distance (and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_no_poses are checked.",
                   -1, -1, 20)

grp_trajectory.add("exact_arc_length", bool_t, 0,
                   "If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.",
                   False)

grp_trajectory.add("publish_feedback", bool_t, 0,
                   "Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)",
                   False)

grp_trajectory.add("control_look_ahead_poses", int_t, 0,
                   "Index of the pose used to extract the velocity command",
                   1, 1, 100)

grp_trajectory.add("prevent_look_ahead_poses_near_goal", int_t, 0,
                   "Prevents control_look_ahead_poses to look within this many poses of the goal in order to prevent overshoot & oscillation when xy_goal_tolerance is very small",
                   0, 0, 20)

grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0,
                   "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.",
                   0, 0, 1)

# ViaPoints
grp_viapoints = gen.add_group("ViaPoints", type="tab")

grp_viapoints.add("global_plan_viapoint_sep", double_t, 0,
                  "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]",
                  -0.1, -0.1, 5.0)

grp_viapoints.add("via_points_ordered", bool_t, 0,
                  "If true, the planner adheres to the order of via-points in the storage container",
                  False)

# Robot
grp_robot = gen.add_group("Robot", type="tab")

grp_robot.add("max_vel_x", double_t, 0,
              "Maximum velocity in the x direction of the robot. May be overruled by the max_vel_trans parameter",
              0.4, 0.01, 100)

grp_robot.add("max_vel_x_backwards", double_t, 0,
              "Maximum translational velocity of the robot for driving backwards",
              0.2, 0.01, 100)

grp_robot.add("max_vel_theta", double_t, 0,
              "Maximum angular velocity of the robot",
              0.3, 0.01, 100)

grp_robot.add("acc_lim_x", double_t, 0,
              "Maximum translational acceleration of the robot",
              0.5, 0.01, 100)

grp_robot.add("acc_lim_theta", double_t, 0,
              "Maximum angular acceleration of the robot",
              0.5, 0.01, 100)

grp_robot.add("is_footprint_dynamic", bool_t, 0,
              "If true, updated the footprint before checking trajectory feasibility",
              False)

grp_robot.add("use_proportional_saturation", bool_t, 0,
              "If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually",
              False)
grp_robot.add("transform_tolerance", double_t, 0,
              "Tolerance when querying the TF Tree for a transformation (seconds)",
              0.5, 0.001, 20)

# Robot/Carlike

grp_robot_carlike = grp_robot.add_group("Carlike", type="hide")

grp_robot_carlike.add("min_turning_radius", double_t, 0,
                      "Minimum turning radius of a carlike robot (diff-drive robot: zero)",
                      0.0, 0.0, 50.0)

grp_robot_carlike.add("wheelbase", double_t, 0,
                      "The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!",
                      1.0, -10.0, 10.0)

grp_robot_carlike.add("cmd_angle_instead_rotvel", bool_t, 0,
                      "Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')",
                      False)

# Robot/Omni
grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide")

grp_robot_omni.add("max_vel_y", double_t, 0,
                   "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!). May be overruled by the max_vel_trans parameter",
                   0.0, 0.0, 100)

grp_robot_omni.add("max_vel_trans", double_t, 0,
                   "Maximum linear velocity of the robot. Will limit max_vel_x and max_vel_y when their linear combination exceeds its value. When set to default 0.0, it will be set equal to max_vel_x.",
                   0.0, 0.0, 100)

grp_robot_omni.add("acc_lim_y", double_t, 0,
                   "Maximum strafing acceleration of the robot",
                   0.5, 0.01, 100)

# GoalTolerance
grp_goal = gen.add_group("GoalTolerance", type="tab")

grp_goal.add("xy_goal_tolerance", double_t, 0,
             "Allowed final euclidean distance to the goal position",
             0.2, 0.001, 10)

grp_goal.add("yaw_goal_tolerance", double_t, 0,
             "Allowed final orientation error to the goal orientation",
             0.1, 0.001, 3.2)

grp_goal.add("free_goal_vel", bool_t, 0,
             "Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)",
             False)

grp_goal.add("trans_stopped_vel", double_t, 0,
             "Below what maximum velocity we consider the robot to be stopped in translation",
             0.1, 0.0)

grp_goal.add("theta_stopped_vel", double_t, 0,
             "Below what maximum rotation velocity we consider the robot to be stopped in rotation",
             0.1, 0.0)

# Obstacles
grp_obstacles = gen.add_group("Obstacles", type="tab")

grp_obstacles.add("min_obstacle_dist", double_t, 0,
                  "Minimum desired separation from obstacles",
                  0.5, 0, 10)

grp_obstacles.add("inflation_dist", double_t, 0,
                  "Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)",
                  0.6, 0, 15)

grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0,
                  "Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)",
                  0.6, 0, 15)

grp_obstacles.add("include_dynamic_obstacles", bool_t, 0,
                  "Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.",
                  False)

grp_obstacles.add("include_costmap_obstacles", bool_t, 0,
                  "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)",
                  True)

grp_obstacles.add("legacy_obstacle_association", bool_t, 0,
                  "If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only 'relevant' obstacles).",
                  False)

grp_obstacles.add("obstacle_association_force_inclusion_factor", double_t, 0,
                  "The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.",
                  1.5, 0.0, 100.0)

grp_obstacles.add("obstacle_association_cutoff_factor", double_t, 0,
                  "See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.",
                  5.0, 1.0, 100.0)

grp_obstacles.add("costmap_obstacles_behind_robot_dist", double_t, 0,
                  "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)",
                  1.5, 0.0, 20.0)

grp_obstacles.add("obstacle_poses_affected", int_t, 0,
                  "The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well",
                  30, 0, 200)

# Obstacle - Velocity ratio parameters
grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles")

grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0,
                                 "Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles",
                                 1, 0, 1)

grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0,
                                 "Distance to a static obstacle for which the velocity should be lower",
                                 0, 0, 10)

grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0,
                                 "Distance to a static obstacle for which the velocity should be higher",
                                 0.5, 0, 10)

# Optimization
grp_optimization = gen.add_group("Optimization", type="tab")

grp_optimization.add("no_inner_iterations", int_t, 0,
                     "Number of solver iterations called in each outerloop iteration",
                     5, 1, 100)

grp_optimization.add("no_outer_iterations", int_t, 0,
                     "Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations",
                     4, 1, 100)

grp_optimization.add("optimization_activate", bool_t, 0, "Activate the optimization", True)

grp_optimization.add("optimization_verbose", bool_t, 0, "Print verbose information", False)

grp_optimization.add("penalty_epsilon", double_t, 0,
                     "Add a small safty margin to penalty functions for hard-constraint approximations",
                     0.05, 0, 1.0)

grp_optimization.add("weight_max_vel_x", double_t, 0,
                     "Optimization weight for satisfying the maximum allowed translational velocity",
                     2, 0, 1000)

grp_optimization.add("weight_max_vel_y", double_t, 0,
                     "Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)",
                     2, 0, 1000)

grp_optimization.add("weight_max_vel_theta", double_t, 0,
                     "Optimization weight for satisfying the maximum allowed angular velocity",
                     1, 0, 1000)

grp_optimization.add("weight_acc_lim_x", double_t, 0,
                     "Optimization weight for satisfying the maximum allowed translational acceleration",
                     1, 0, 1000)

grp_optimization.add("weight_acc_lim_y", double_t, 0,
                     "Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)",
                     1, 0, 1000)

grp_optimization.add("weight_acc_lim_theta", double_t, 0,
                     "Optimization weight for satisfying the maximum allowed angular acceleration",
                     1, 0, 1000)

grp_optimization.add("weight_kinematics_nh", double_t, 0,
                     "Optimization weight for satisfying the non-holonomic kinematics",
                     1000, 0, 10000)

grp_optimization.add("weight_kinematics_forward_drive", double_t, 0,
                     "Optimization weight for forcing the robot to choose only forward directions (positive transl. "
                     "velocities, only diffdrive robot)",
                     1, 0, 10000)

grp_optimization.add("weight_kinematics_turning_radius", double_t, 0,
                     "Optimization weight for enforcing a minimum turning radius (carlike robots)",
                     1, 0, 1000)

grp_optimization.add("weight_optimaltime", double_t, 0,
                     "Optimization weight for contracting the trajectory w.r.t. transition time",
                     1, 0, 1000)

grp_optimization.add("weight_shortest_path", double_t, 0,
                     "Optimization weight for contracting the trajectory w.r.t. path length",
                     0, 0, 100)

grp_optimization.add("weight_obstacle", double_t, 0,
                     "Optimization weight for satisfying a minimum seperation from obstacles",
                     50, 0, 1000)

grp_optimization.add("weight_inflation", double_t, 0,
                     "Optimization weight for the inflation penalty (should be small)",
                     0.1, 0, 10)

grp_optimization.add("weight_dynamic_obstacle", double_t, 0,
                     "Optimization weight for satisfying a minimum seperation from dynamic obstacles",
                     50, 0, 1000)

grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0,
                     "Optimization weight for the inflation penalty of dynamic obstacles (should be small)",
                     0.1, 0, 10)

grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0,
                     "Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle",
                     0, 0, 1000)

grp_optimization.add("weight_viapoint", double_t, 0,
                     "Optimization weight for minimizing the distance to via-points",
                     1, 0, 1000)

grp_optimization.add("weight_adapt_factor", double_t, 0,
                     "Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.",
                     2, 1, 100)

grp_optimization.add("obstacle_cost_exponent", double_t, 0,
                     "Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)",
                     1, 0.01, 100)
# Homotopy Class Planner
grp_hcp = gen.add_group("HCPlanning", type="tab")
grp_hcp.add("enable_multithreading", bool_t, 0,
            "Activate multiple threading for planning multiple trajectories in parallel", True)
grp_hcp.add("max_number_classes", int_t, 0,
            "Specify the maximum number of allowed alternative homotopy classes (limits computational effort)", 5, 1,
            100)
grp_hcp.add("max_number_plans_in_current_class", int_t, 0,
            "Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes",
            1, 1, 10)
grp_hcp.add("selection_cost_hysteresis", double_t, 0,
            "Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor)",
            1.0, 0, 2)
grp_hcp.add("selection_prefer_initial_plan", double_t, 0,
            "Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)",
            0.95, 0, 1)
grp_hcp.add("selection_obst_cost_scale", double_t, 0,
            "Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)",
            2.0, 0, 1000)
grp_hcp.add("selection_viapoint_cost_scale", double_t, 0,
            "Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)",
            1.0, 0, 100)
grp_hcp.add("selection_alternative_time_cost", bool_t, 0,
            "If true, time cost is replaced by the total transition time.", False)
grp_hcp.add("selection_dropping_probability", double_t, 0,
            "At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.",
            0.0, 0.0, 1.0)
grp_hcp.add("switching_blocking_period", double_t, 0,
            "Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed",
            0.0, 0.0, 60)
grp_hcp.add("roadmap_graph_no_samples", int_t, 0,
            "Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off",
            15, 1, 100)
grp_hcp.add("roadmap_graph_area_width", double_t, 0,
            "Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)",
            5, 0.1, 20)
grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0,
            "The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!)",
            1.0, 0.5, 2)
grp_hcp.add("h_signature_prescaler", double_t, 0,
            "Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<H<=1)",
            1, 0.2, 1)
grp_hcp.add("h_signature_threshold", double_t, 0,
            "Two h-signuteres are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold",
            0.1, 0, 1)
grp_hcp.add("obstacle_heading_threshold", double_t, 0,
            "Specify the value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration)",
            0.45, 0, 1)
grp_hcp.add("viapoints_all_candidates", bool_t, 0,
            "If true, all trajectories of different topologies are attached to the set of via-points, otherwise only the trajectory sharing the same one as the initial/global plan is attached (no effect in test_optim_node).",
            True)
grp_hcp.add("visualize_hc_graph", bool_t, 0, "Visualize the graph that is created for exploring new homotopy classes",
            False)

# Recovery
grp_recovery = gen.add_group("Recovery", type="tab")
grp_recovery.add("shrink_horizon_backup", bool_t, 0,
                 "Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues.",
                 True)
grp_recovery.add("oscillation_recovery", bool_t, 0,
                 "Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards).",
                 True)
grp_recovery_divergence = grp_recovery.add_group("Divergence Detection", type="hide")
grp_recovery_divergence.add("divergence_detection_enable", bool_t, 0, "True to enable divergence detection.", False)
grp_recovery_divergence.add("divergence_detection_max_chi_squared", double_t, 0,
                            "Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged.",
                            10, 0, 100)

exit(gen.generate("teb_local_planner", "teb_local_planner", "TebLocalPlannerReconfigure"))
